﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Windows;
using System.Windows.Media.Media3D;
using System.Windows.Threading;
using WiimoteLib;
using Point = System.Windows.Point;

namespace WpfHeadtracking
{
    /// <summary>
    /// Implements the Nintendo Wiimote as Headtracking Provider.
    /// Largely based on Johnny Lee's WiiDesktopVR: http://www.cs.cmu.edu/~johnny/projects/wii/
    /// Some of his original comments were left in the code.
    /// </summary>
    public class WiimoteHeadtrackingProvider : DependencyObject, IHeadtrackingProvider
    {
        #region Constants

        // Wiimote camera Width and Height in pixels
        private const double cameraHeight = 768.0;
        private const double cameraWidth = 1024.0;
        private const double movementScaling = 1;

        // the Wiimote camera field of vision is 45 degrees = PI/4
        private const double radiansPerPixel = (Math.PI / 4) / cameraWidth;

        #endregion

        #region Fields

        private readonly Wiimote wiimote = new Wiimote();


        private double cameraVerticalAngle; // begins assuming the camera is pointing straight forward
        private Point3D headPosition = new Point3D(0, 0, 0);
        private bool isHeadtracking;
        private double relativeVerticalAngle; // current head position view angle

        #endregion

        #region Properties

        public static readonly DependencyProperty DotDistanceInMMProperty =
    DependencyProperty.Register("DotDistanceInMM", typeof(double), typeof(WiimoteHeadtrackingProvider),
                                new UIPropertyMetadata(8.5 * 25.4)); // Width of the wii sensor bar

        public static readonly DependencyProperty IsWiimoteAboveScreenProperty =
            DependencyProperty.Register("IsWiimoteAboveScreen", typeof(bool), typeof(WiimoteHeadtrackingProvider),
                                        new UIPropertyMetadata(false));

        public static readonly DependencyProperty ScreenHeightInMMProperty =
            DependencyProperty.Register("ScreenHeightInMM", typeof(double), typeof(WiimoteHeadtrackingProvider),
                                        new UIPropertyMetadata(289.56)); // A common 19 inch monitor

        public double DotDistanceInMM
        {
            get
            {
                var value = (double)DotDistanceInMMProperty.DefaultMetadata.DefaultValue;
                Dispatcher.Invoke(DispatcherPriority.Normal, (Action)(() =>
                {
                    value = (double)GetValue(DotDistanceInMMProperty);
                }));
                return value;
            }
            set
            {
                Dispatcher.BeginInvoke(DispatcherPriority.Normal, (Action)(() => SetValue(DotDistanceInMMProperty, value)));
            }
        }

        public double ScreenHeightInMM
        {
            get
            {
                var value = (double)ScreenHeightInMMProperty.DefaultMetadata.DefaultValue;
                Dispatcher.Invoke(DispatcherPriority.Normal, (Action)(() =>
                {
                    value = (double)GetValue(ScreenHeightInMMProperty);
                }));
                return value;
            }
            set
            {
                Dispatcher.BeginInvoke(DispatcherPriority.Normal, (Action)(() => SetValue(ScreenHeightInMMProperty, value)));
            }
        }

        public bool IsWiimoteAboveScreen
        {
            get
            {
                var value = (bool)IsWiimoteAboveScreenProperty.DefaultMetadata.DefaultValue;
                Dispatcher.Invoke(DispatcherPriority.Normal, (Action)(() =>
                {
                    value = (bool)GetValue(IsWiimoteAboveScreenProperty);
                }));
                return value;
            }
            set
            {
                Dispatcher.BeginInvoke(DispatcherPriority.Normal, (Action)(() => SetValue(IsWiimoteAboveScreenProperty, value)));
            }
        }

        #endregion

        #region IHeadtrackingProvider Members

        public Point3D HeadPosition
        {
            get { return headPosition; }
        }

        public bool IsHeadtracking
        {
            get { return isHeadtracking; }
        }

        public void SetHeadOrigin()
        {
            // zeros the head position and computes the camera tilt
            double angle = Math.Acos(.5 / headPosition.Z) - Math.PI / 2; // angle of head to screen
            if (!IsWiimoteAboveScreen)
                angle = -angle;
            cameraVerticalAngle = ((angle - relativeVerticalAngle)); // absolute camera angle
        }

        public void StartHeadtracking()
        {
            try
            {
                wiimote.WiimoteChanged += wm_WiimoteChanged;
                wiimote.Connect();
                wiimote.SetLEDs(true, false, false, false);
                wiimote.SetReportType(InputReport.IRAccel, true);
            }
            catch (WiimoteException)
            {
                // Fail gracefully
                // TODO: Better fail scenarios
                StopHeadtracking();
            }
        }

        public void StopHeadtracking()
        {
            isHeadtracking = false;
            wiimote.WiimoteChanged -= wm_WiimoteChanged;
            wiimote.Disconnect();
        }

        public event PropertyChangedEventHandler HeadPositionChanged;

        protected void OnHeadPositionChanged()
        {
            PropertyChangedEventHandler handler = HeadPositionChanged;
            if (handler != null)
            {
                handler(this, new PropertyChangedEventArgs("HeadPosition"));
            }
        }

        #endregion

        private void wm_WiimoteChanged(object sender, WiimoteChangedEventArgs args)
        {
            WiimoteState ws = args.WiimoteState;

            if (ws == null) return;

            var wiimoteVisiblePoints = new Point[4];
            Point firstPoint, secondPoint;
            int visiblePoints = 0;

            IEnumerable<IRSensor> foundSensors = from IRSensor s in ws.IRState.IRSensors
                                                 where (s.Found)
                                                 select s;
            foreach (IRSensor s in foundSensors)
            {
                wiimoteVisiblePoints[visiblePoints] = new Point(s.RawPosition.X, s.RawPosition.Y);

                visiblePoints++;
            }

            if (visiblePoints == 2)
            {
                firstPoint = wiimoteVisiblePoints[0];
                secondPoint = wiimoteVisiblePoints[1];

                double dx = firstPoint.X - secondPoint.X;
                double dy = firstPoint.Y - secondPoint.Y;
                double pointDist = Math.Sqrt(dx * dx + dy * dy);

                double angle = radiansPerPixel * pointDist / 2;

                headPosition.Z = movementScaling * ((DotDistanceInMM / 2) / Math.Tan(angle)) / ScreenHeightInMM;

                double avgX = (firstPoint.X + secondPoint.X) / 2.0;
                double avgY = (firstPoint.Y + secondPoint.Y) / 2.0;

                //should calculate based on distance

                headPosition.X =
                    (movementScaling * Math.Sin(radiansPerPixel * (avgX - cameraWidth / 2)) * headPosition.Z);

                relativeVerticalAngle = (avgY - cameraHeight / 2) * radiansPerPixel; //relative angle to camera axis

                if (IsWiimoteAboveScreen)
                    headPosition.Y = .5f +
                                     (movementScaling * Math.Sin(relativeVerticalAngle + cameraVerticalAngle) *
                                      headPosition.Z);
                else
                    headPosition.Y = -.5f +
                                     (movementScaling * Math.Sin(relativeVerticalAngle + cameraVerticalAngle) *
                                      headPosition.Z);

                isHeadtracking = true;
                OnHeadPositionChanged();
            }
            else
                isHeadtracking = false;
        }

    }
}